#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkModifiedBSPTree.h>
#include <vtkSphereSource.h>
#include <vtkPolyDataReader.h>
#include "vtkDoubleArray.h"
#include <vtkPointData.h>
#include <vtkMath.h>
#include "vtkPolyDataWriter.h"


/*
./ThicknessCalculator  convexHull_SPHARM8.vtk  midMyocardium_SPHARM8.vtk  walloutput_SPHARM8.vtk  midMyocardium_Thickness8.vtk
./ThicknessCalculator  convexHull_SPHARM20.vtk  midMyocardium_SPHARM20.vtk  walloutput_SPHARM20.vtk  midMyocardium_Thickness20.vtk
./Show  midMyocardium_Thickness8.vtk 512 512 Thickness
./Show  midMyocardium_Thickness20.vtk 512 512 Thickness
*/

int main(int argc, char * argv[])
{
// Load InnerMyocardium
	vtkPolyDataReader *innerReader = vtkPolyDataReader::New(); 
	innerReader->SetFileName(argv[1]);
	innerReader->Update();
	vtkSmartPointer < vtkPolyData > innerPolyData = innerReader->GetOutput();
// Create the bspTree for innerSurface
  vtkSmartPointer<vtkModifiedBSPTree> innerTree = vtkSmartPointer<vtkModifiedBSPTree>::New();
  innerTree->SetDataSet(innerReader->GetOutput());
  innerTree->BuildLocator();

// Load midMyocardium
	vtkPolyDataReader *midReader = vtkPolyDataReader::New(); 
	midReader->SetFileName(argv[2]);
	midReader->Update();
	vtkSmartPointer < vtkPolyData > midPolyData = midReader->GetOutput();

// Load OuterMyocardium
	vtkPolyDataReader *outerReader = vtkPolyDataReader::New(); 
	outerReader->SetFileName(argv[3]);
	outerReader->Update();
	vtkSmartPointer < vtkPolyData > outerPolyData = outerReader->GetOutput();
// Create the bspTree for outerSurface
  vtkSmartPointer<vtkModifiedBSPTree> outerTree = vtkSmartPointer<vtkModifiedBSPTree>::New();
  outerTree->SetDataSet(outerReader->GetOutput());
  outerTree->BuildLocator();

 

  ///////// Get Point Normals ///////////
  vtkSmartPointer<vtkDoubleArray> pointNormalsRetrieved = vtkDoubleArray::SafeDownCast(midPolyData->GetPointData()->GetNormals());
  if(pointNormalsRetrieved)
    {
    int n_vert = pointNormalsRetrieved->GetNumberOfTuples();
    std::cout << "There are " << n_vert << " point normals." << std::endl;

double * thickness = new double[n_vert];
        vtkDoubleArray* Thickness = vtkDoubleArray::New();
        Thickness->SetName("Thickness");
        Thickness->SetArray(thickness, n_vert, 1);

  // BSPTree Inputs: pStart, pEnd
  double tolerance = .001;
 
  // BSPTree Outputs:
  double t; // Parametric coordinate of intersection (0 (corresponding to p1) to 1 (corresponding to p2))
  double innerPoint[3], outerPoint[3]; // The coordinate of the intersection
  double pcoords[3];
  int subId;
 
    for(vtkIdType i = 0; i < n_vert; i++)
      {
      double p[3], pNormal[3], pStart[3], pEnd[3], halfLength=10.0;
      midPolyData->GetPoint(i, p);
      //std::cout << "Point " << i << ": " << p[0] << " " << p[1] << " " << p[2] << std::endl;
      pointNormalsRetrieved->GetTuple(i, pNormal);
      //std::cout << "Point normal " << i << ": " << pNormal[0] << " " << pNormal[1] << " " << pNormal[2] << std::endl;
      pStart[0] = p[0]-halfLength*pNormal[0];
      pStart[1] = p[1]-halfLength*pNormal[1];
      pStart[2] = p[2]-halfLength*pNormal[2];
      //std::cout << "Start Point " << i << ": " << pStart[0] << " " << pStart[1] << " " << pStart[2] << std::endl;
      pEnd[0] = p[0]+halfLength*pNormal[0];
      pEnd[1] = p[1]+halfLength*pNormal[1];
      pEnd[2] = p[2]+halfLength*pNormal[2];
      //std::cout << "End Point " << i << ": " << pEnd[0] << " " << pEnd[1] << " " << pEnd[2] << std::endl;

	  // Note: for a typical use case (ray-triangle intersection), pcoords and subId will not be used
	  // iD is 0 if no intersections were found
	  vtkIdType iD = innerTree->IntersectWithLine(pStart, pEnd, tolerance, t, innerPoint, pcoords, subId);
	//  std::cout << "iD: " << iD << std::endl;
	//  std::cout << "t: " << t << std::endl;
	//  std::cout << "Inner Intersection Point: " << innerPoint[0] << " " << innerPoint[1] << " " << innerPoint[2] << std::endl;
	//  std::cout << "pcoords: " << pcoords[0] << " " << pcoords[1] << " " << pcoords[2] << std::endl;
	//  std::cout << "subId: " << subId << std::endl;

	  iD = outerTree->IntersectWithLine(pStart, pEnd, tolerance, t, outerPoint, pcoords, subId);
	//  std::cout << "iD: " << iD << std::endl;
	//  std::cout << "t: " << t << std::endl;
	//  std::cout << "Outer Intersection Point: " << outerPoint[0] << " " << outerPoint[1] << " " << outerPoint[2] << std::endl<< std::endl;
	//  std::cout << "pcoords: " << pcoords[0] << " " << pcoords[1] << " " << pcoords[2] << std::endl;
	//  std::cout << "subId: " << subId << std::endl;

	// Find the squared distance between the points.
	  double squaredDistance = vtkMath::Distance2BetweenPoints(innerPoint, outerPoint);

	  // Take the square root to get the Euclidean distance between the points.
	  double distance = sqrt(squaredDistance);

	  // Output the results.
	  std::cout << "Distance = " << distance << std::endl;

	  thickness[i] = distance;
      }
 
midPolyData->GetPointData()->SetScalars(Thickness);
    vtkPolyDataWriter *vtkwriter;
    vtkwriter = vtkPolyDataWriter::New();
    vtkwriter->SetInput(midPolyData);
    vtkwriter->SetFileName(argv[4]);
    vtkwriter->Write();

   
    }//end if(pointNormalsRetrieved)
  else
    {
    std::cout << "No point normals." << std::endl;
    }
 


  return EXIT_SUCCESS;
}
